#!/usr/bin/env python
PACKAGE = "franka_example_controllers"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

controller_gains = gen.add_group("Controller_Gains", type="apply")
controller_gains.add("leader_damping_scaling", double_t, 0,
                     "Factor to be multiplied with initial leader d_gains",
                     1.0, 0.0, 8.0)
controller_gains.add(
    "follower_stiffness_scaling", double_t, 0,
    "Factor to be multiplied with initial p-gains of follower arm, damping will also be adapted",
    1.0, 0.1, 1.3)

force_feedback = gen.add_group("Force_Feedback")
force_feedback.add("force_feedback_idle", double_t, 0,
                   "Applied feedback force when leader arm is idle", 0.5, 0.0,
                   1.0)
force_feedback.add("force_feedback_guiding", double_t, 0,
                   "Applied feedback force when leader arm is guided", 0.98,
                   0.0, 1.0)

contact_param = gen.add_group("Cartesian_Contact")
contact_param.add(
    "leader_contact_force_threshold", double_t, 0,
    "Force threshold at leader endeffector to determine whether the arm is in contact.",
    4.0, 0.1, 10.0)
contact_param.add(
    "follower_contact_force_threshold", double_t, 0,
    "Force threshold at follower endeffector to determine whether the arm is in contact.",
    5.0, 0.1, 10.0)

dq_max = gen.add_group("Max_Velocities")
dq_max_lower_tab = dq_max.add_group("Dq_MaxLower", type="tab")
dq_max_lower = dq_max_lower_tab.add_group("dq_max_lower", type="apply")

dq_max_lower.add("dq_l_1", double_t, 0, "Description", 0.8, 0.1, 2.17)
dq_max_lower.add("dq_l_2", double_t, 0, "Description", 0.8, 0.1, 2.17)
dq_max_lower.add("dq_l_3", double_t, 0, "Description", 0.8, 0.1, 2.17)
dq_max_lower.add("dq_l_4", double_t, 0, "Description", 0.8, 0.1, 2.17)
dq_max_lower.add("dq_l_5", double_t, 0, "Description", 2.5, 0.1, 2.6)
dq_max_lower.add("dq_l_6", double_t, 0, "Description", 2.5, 0.1, 2.6)
dq_max_lower.add("dq_l_7", double_t, 0, "Description", 2.5, 0.1, 2.6)

dq_max_upper_tab = dq_max.add_group("Dq_MaxUpper", type="tab")
dq_max_upper = dq_max_upper_tab.add_group("dq_max_upper", type="apply")
dq_max_upper.add("dq_u_1", double_t, 0, "Description", 2.0, 0.1, 2.17)
dq_max_upper.add("dq_u_2", double_t, 0, "Description", 2.0, 0.1, 2.17)
dq_max_upper.add("dq_u_3", double_t, 0, "Description", 2.0, 0.1, 2.17)
dq_max_upper.add("dq_u_4", double_t, 0, "Description", 2.0, 0.1, 2.17)
dq_max_upper.add("dq_u_5", double_t, 0, "Description", 2.5, 0.1, 2.6)
dq_max_upper.add("dq_u_6", double_t, 0, "Description", 2.5, 0.1, 2.6)
dq_max_upper.add("dq_u_7", double_t, 0, "Description", 2.5, 0.1, 2.6)

ddq_max = gen.add_group("Max_Acceleration")
ddq_max_lower_tab = ddq_max.add_group("Ddq_MaxLower", type="tab")
ddq_max_lower = ddq_max_lower_tab.add_group("ddq_max_lower", type="apply")
ddq_max_lower.add("ddq_l_1", double_t, 0, "Description", 0.8, 0.1, 15.0)
ddq_max_lower.add("ddq_l_2", double_t, 0, "Description", 0.8, 0.1, 7.5)
ddq_max_lower.add("ddq_l_3", double_t, 0, "Description", 0.8, 0.1, 10.0)
ddq_max_lower.add("ddq_l_4", double_t, 0, "Description", 0.8, 0.1, 12.5)
ddq_max_lower.add("ddq_l_5", double_t, 0, "Description", 10.0, 0.1, 15.0)
ddq_max_lower.add("ddq_l_6", double_t, 0, "Description", 10.0, 0.1, 20.0)
ddq_max_lower.add("ddq_l_7", double_t, 0, "Description", 10.0, 0.1, 20.0)

ddq_max_upper_tab = ddq_max.add_group("Ddq_MaxUpper", type="tab")
ddq_max_upper = ddq_max_upper_tab.add_group("ddq_max_upper", type="apply")
ddq_max_upper.add("ddq_u_1", double_t, 0, "Description", 6.0, 0.1, 15.0)
ddq_max_upper.add("ddq_u_2", double_t, 0, "Description", 6.0, 0.1, 7.5)
ddq_max_upper.add("ddq_u_3", double_t, 0, "Description", 6.0, 0.1, 10.0)
ddq_max_upper.add("ddq_u_4", double_t, 0, "Description", 6.0, 0.1, 12.5)
ddq_max_upper.add("ddq_u_5", double_t, 0, "Description", 15.0, 0.1, 15.0)
ddq_max_upper.add("ddq_u_6", double_t, 0, "Description", 20.0, 0.1, 20.0)
ddq_max_upper.add("ddq_u_7", double_t, 0, "Description", 20.0, 0.1, 20.0)

exit(gen.generate(PACKAGE, "teleop_param", "teleop_param"))
